#include "AeSerials.h"


CProtoSerial::CProtoSerial()
{
}

CProtoSerial::~CProtoSerial()
{
}

// 0xFB 0xBF len(1B) cmd(1B) params(N*B) checksum(1B) 0xED
// checksum = LOW_B(len+cmd+params)
uint8_t CProtoSerial::Checksum(uint32_t size, const uint8_t *data)
{
	uint32_t sum = 0;
	for (int i = 2; i < size - 2; i++) {
		sum += data[i];
	}
	return (sum & 0xFF);
}

// Pack: 0x00(1B) // 7bytes
// Unpack: version(4B) // 10bytes
AeRet_t CProtoSerial::PackGetVersion(uint32_t &size, uint8_t *data)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_GET_VERSION);
	putU8(pos, data, 0x00);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackGetVersion(uint32_t size, const uint8_t *data, uint32_t &version)
{
	if (!check(size, data, 10)) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 4;
	getU32(pos, data, version);

	return AE_RET_SUCCESS;
}

// Pack: 0x00(1B) // 7bytes
// Unpack: total(4B) used(4B) // 14bytes
AeRet_t CProtoSerial::PackGetStorage(uint32_t &size, uint8_t *data)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_GET_STORAGE);
	putU8(pos, data, 0x00);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackGetStorage(uint32_t size, const uint8_t *data, uint32_t &total, uint32_t &used)
{
	if (!check(size, data, 14)) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 4;
	getU32(pos, data, total);
	getU32(pos, data, used);

	return AE_RET_SUCCESS;
}

// Pack: 0x00(1B) // 7bytes
// Unpack: ret(1B) // 7bytes
AeRet_t CProtoSerial::PackGetActions(uint32_t &size, uint8_t *data)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_GET_ACTIONS);
	putU8(pos, data, 0x00);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackGetActions(uint32_t size, const uint8_t *data)
{
	if (!check(size, data, 7)) {
		return AE_RET_ERROR;
	}

	return (AeRet_t)data[4];
}

// Pack: action(AE_PATH_MAX) AeActHeader_t // 6+AE_PATH_MAX+sizeof(AeActHeader_t) bytes
// Unpack: None
AeRet_t CProtoSerial::PackGetactSending(uint32_t &size, uint8_t *data, uint32_t actLen, const char *actStr, const AeActHeader_t &header)
{
	uint8_t len = 6 + AE_PATH_MAX + sizeof(AeActHeader_t);
	if (size < len) {
		return AE_RET_ERROR;
	}
	if (actLen >= AE_PATH_MAX) {
		return AE_RET_PARAMS;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_GETACT_SENDING);
	putString(pos, data, actLen, actStr);
	memcpy(data + pos, &header, sizeof(header));
	pos += sizeof(header);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackGetactSending(uint32_t size, const uint8_t *data, char *actStr, AeActHeader_t &header)
{
	if (!check(size, data, 6 + AE_PATH_MAX + sizeof(AeActHeader_t))) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 4;
	getString(pos, data, actStr);
	memcpy(&header, data + pos, sizeof(header));
	pos += sizeof(header);

	return AE_RET_SUCCESS;
}

// Pack: 0x00(1B) // 7bytes
// Unpack: None
AeRet_t CProtoSerial::PackGetactFinished(uint32_t &size, uint8_t *data)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_GETACT_FINISHED);
	putU8(pos, data, 0x00);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackGetactFinished(uint32_t size, const uint8_t *data)
{
	if (!check(size, data, 7)) {
		return AE_RET_ERROR;
	}

	return AE_RET_SUCCESS;
}

// Pack: action(AE_PATH_MAX) // 6+AE_PATH_MAX bytes
// Unpack: ret(1B) // 7bytes
AeRet_t CProtoSerial::PackPlayAction(uint32_t &size, uint8_t *data, uint32_t actLen, const char *actStr)
{
	uint8_t len = 6 + AE_PATH_MAX;
	if (size < len) {
		return AE_RET_ERROR;
	}
	if (actLen >= AE_PATH_MAX) {
		return AE_RET_PARAMS;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_PLAY_ACTION);
	putString(pos, data, actLen, actStr);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackPlayAction(uint32_t size, const uint8_t *data)
{
	if (!check(size, data, 7)) {
		return AE_RET_ERROR;
	}

	return (AeRet_t)data[4];
}

// Pack: total(4B) played(4B) // 14bytes
// Unpack: None
AeRet_t CProtoSerial::PackPlayactSending(uint32_t &size, uint8_t *data, uint32_t total, uint32_t played)
{
	uint8_t len = 14;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_PLAYACT_SENDING);
	putU32(pos, data, total);
	putU32(pos, data, played);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackPlayactSending(uint32_t size, const uint8_t *data, uint32_t &total, uint32_t &played)
{
	if (!check(size, data, 14)) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 4;
	getU32(pos, data, total);
	getU32(pos, data, played);

	return AE_RET_SUCCESS;
}

// Pack: 0x00(1B) // 7bytes
// Ret: None
AeRet_t CProtoSerial::PackPlayactFinished(uint32_t &size, uint8_t *data)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_PLAYACT_FINISHED);
	putU8(pos, data, 0x00);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackPlayactFinished(uint32_t size, const uint8_t *data)
{
	if (!check(size, data, 7)) {
		return AE_RET_ERROR;
	}

	return AE_RET_SUCCESS;
}

// Pack: 0x00(1B) // 7bytes
// Unpack: 0x00(1B) // 7bytes
AeRet_t CProtoSerial::PackStopAction(uint32_t &size, uint8_t *data)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_STOP_ACTION);
	putU8(pos, data, 0x00);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackStopAction(uint32_t size, const uint8_t *data)
{
	if (!check(size, data, 7)) {
		return AE_RET_ERROR;
	}

	return AE_RET_SUCCESS;
}

// Pack: (0x00-pause/0x01-resume)(1B) // 7bytes
// Unpack: 0x00(1B) // 7bytes
AeRet_t CProtoSerial::PackCtrlAction(uint32_t &size, uint8_t *data, bool pause)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_CTRL_ACTION);
	putU8(pos, data, pause ? 0x00 : 0x01);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackCtrlAction(uint32_t size, const uint8_t *data)
{
	if (!check(size, data, 7)) {
		return AE_RET_ERROR;
	}

	return AE_RET_SUCCESS;
}

// Pack: action(AE_PATH_MAX) // 6+AE_PATH_MAX bytes
// Unpack: ret(1B) action(AE_PATH_MAX) // 7+AE_PATH_MAX bytes
AeRet_t CProtoSerial::PackDeleteAction(uint32_t &size, uint8_t *data, uint32_t actLen, const char *actStr)
{
	uint8_t len = 6 + AE_PATH_MAX;
	if (size < len) {
		return AE_RET_ERROR;
	}
	if (actLen >= AE_PATH_MAX) {
		return AE_RET_PARAMS;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_DELETE_ACTION);
	putString(pos, data, actLen, actStr);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackDeleteAction(uint32_t size, const uint8_t *data, char *actStr)
{
	if (!check(size, data, 7 + AE_PATH_MAX)) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 4;
	uint8_t ret;
	getU8(pos, data, ret);
	getString(pos, data, actStr);
	return (AeRet_t)ret;
}

// Pack: action(AE_PATH_MAX) AeActHeader_t // 6+AE_PATH_MAX+sizeof(AeActHeader_t) bytes
// Unpack: ret(1B) // 7bytes
AeRet_t CProtoSerial::PackUploadAction(uint32_t &size, uint8_t *data, uint32_t actLen, const char *actStr, const AeActHeader_t &header)
{
	uint8_t len = 6 + AE_PATH_MAX + sizeof(AeActHeader_t);
	if (size < len) {
		return AE_RET_ERROR;
	}
	if (actLen >= AE_PATH_MAX) {
		return AE_RET_PARAMS;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_UPLOAD_ACTION);
	putString(pos, data, actLen, actStr);
	memcpy(data + pos, &header, sizeof(header));
	pos += sizeof(header);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackUploadAction(uint32_t size, const uint8_t *data)
{
	if (!check(size, data, 7)) {
		return AE_RET_ERROR;
	}

	return (AeRet_t)data[4];
}

// Pack: action(AE_PATH_MAX) AeActPose_t // 6+AE_PATH_MAX+sizeof(AeActPose_t) bytes
// Unpack: ret(1B) seq(4B) // 11bytes
AeRet_t CProtoSerial::PackUploadactSending(uint32_t &size, uint8_t *data, uint32_t actLen, const char *actStr, const AeActPose_t &pose)
{
	uint8_t len = 6 + AE_PATH_MAX + sizeof(AeActPose_t);
	if (size < len) {
		return AE_RET_ERROR;
	}
	if (actLen >= AE_PATH_MAX) {
		return AE_RET_PARAMS;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_UPLOADACT_SENDING);
	putString(pos, data, actLen, actStr);
	memcpy(data + pos, &pose, sizeof(pose));
	pos += sizeof(pose);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackUploadactSending(uint32_t size, const uint8_t *data, uint32_t &seq)
{
	if (!check(size, data, 11)) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 4;
	uint8_t ret;
	getU8(pos, data, ret);
	getU32(pos, data, seq);
	return (AeRet_t)ret;
}

// Req: action(AE_PATH_MAX) // 6+AE_PATH_MAX bytes
// Ret: ret(1B) // 7bytes
AeRet_t CProtoSerial::PackUploadactFinished(uint32_t &size, uint8_t *data, uint32_t actLen, const char *actStr)
{
	uint8_t len = 6 + AE_PATH_MAX;
	if (size < len) {
		return AE_RET_ERROR;
	}
	if (actLen >= AE_PATH_MAX) {
		return AE_RET_PARAMS;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_UPLOADACT_FINISHED);
	putString(pos, data, actLen, actStr);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackUploadactFinished(uint32_t size, const uint8_t *data)
{
	if (!check(size, data, 7)) {
		return AE_RET_ERROR;
	}

	return (AeRet_t)data[4];
}

// Pack: (0x00-mute/0x01-normal)(1B) // 7bytes
// Unpack: 0x00 // 7bytes
AeRet_t CProtoSerial::PackOffOnVoice(uint32_t &size, uint8_t *data, bool on)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_OFFON_VOICE);
	putU8(pos, data, on ? 0x01 : 0x00);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackOffOnVoice(uint32_t size, const uint8_t *data)
{
	if (!check(size, data, 7)) {
		return AE_RET_ERROR;
	}

	return AE_RET_SUCCESS;
}

// Pack: volume(1B) // 7bytes
// Unpack: 0x00(1B) // 7bytes
AeRet_t CProtoSerial::PackSetVolume(uint32_t &size, uint8_t *data, uint8_t volume)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_SET_VOLUME);
	putU8(pos, data, volume);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackSetVolume(uint32_t size, const uint8_t *data)
{
	if (!check(size, data, 7)) {
		return AE_RET_ERROR;
	}

	return AE_RET_SUCCESS;
}

// Pack: dir(1B) idx(1B) // 8bytes
// Unpack: 0x00(1B) // 7bytes
AeRet_t CProtoSerial::PackPlayMusic(uint32_t &size, uint8_t *data, uint8_t dir, uint8_t idx)
{
	uint8_t len = 8;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_PLAY_MUSIC);
	putU8(pos, data, dir);
	putU8(pos, data, idx);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackPlayMusic(uint32_t size, const uint8_t *data)
{
	if (!check(size, data, 7)) {
		return AE_RET_ERROR;
	}

	return AE_RET_SUCCESS;
}

// Pack: 0x00(1B) // 7bytes
// Unpack: 0x00(1B) // 7bytes
AeRet_t CProtoSerial::PackStopMusic(uint32_t &size, uint8_t *data)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_STOP_MUSIC);
	putU8(pos, data, 0x00);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackStopMusic(uint32_t size, const uint8_t *data)
{
	if (!check(size, data, 7)) {
		return AE_RET_ERROR;
	}

	return AE_RET_SUCCESS;
}

// Pack: (0x00-pause/0x01-resume)(1B) // 7bytes
// Unpack: 0x00(1B) // 7bytes
AeRet_t CProtoSerial::PackCtrlMusic(uint32_t &size, uint8_t *data, bool pause)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_CTRL_MUSIC);
	putU8(pos, data, pause ? 0x00 : 0x01);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackCtrlMusic(uint32_t size, const uint8_t *data)
{
	if (!check(size, data, 7)) {
		return AE_RET_ERROR;
	}

	return AE_RET_SUCCESS;
}

// Pack: idx(1B) (0x00-off/0x01-on)(1B) // 8bytes
// Unpack: idx(1B) 0x00(1B) // 8bytes
AeRet_t CProtoSerial::PackSetServoLed(uint32_t &size, uint8_t *data, uint8_t idx, bool on)
{
	uint8_t len = 8;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_SET_SERVO_LED);
	putU8(pos, data, idx);
	putU8(pos, data, on ? 0x01 : 0x00);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackSetServoLed(uint32_t size, const uint8_t *data, uint8_t &idx)
{
	if (!check(size, data, 8)) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 4;
	idx = data[pos++];

	return AE_RET_SUCCESS;
}

// Pack: (0x00-off/0x01-on)(1B) // 7bytes
// Unpack: 0x00(1B) // 7bytes
AeRet_t CProtoSerial::PackSetServosLed(uint32_t &size, uint8_t *data, bool on)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_SET_SERVOS_LED);
	putU8(pos, data, on ? 0x01 : 0x00);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackSetServosLed(uint32_t size, const uint8_t *data)
{
	if (!check(size, data, 7)) {
		return AE_RET_ERROR;
	}

	return AE_RET_SUCCESS;
}

// Pack: idx(1B) Angle(2B) Time(1B) LckTime(1B) // 11bytes
// Unpack: idx(1B) ret(1B) // 8bytes
// Time unit is in 20ms!!!
AeRet_t CProtoSerial::PackSetServoAngle(uint32_t &size, uint8_t *data, uint8_t idx, uint16_t angle,
	uint32_t msec, uint32_t lckmsec)
{
	uint8_t len = 11;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_SET_SERVO_ANGLE);
	putU8(pos, data, idx);
	putU16(pos, data, angle);
	putU8(pos, data, (msec / 20) & 0xFF);
	putU8(pos, data, (lckmsec / 20) & 0xFF);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackSetServoAngle(uint32_t size, const uint8_t *data, uint8_t &idx)
{
	if (!check(size, data, 8)) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 4;
	idx = data[pos++];

	return (AeRet_t)data[5];
}

// Pack: angles(AE_SERVO_MAX*2B) time(1B) lcktime(1B) // 8+2*AE_SERVO_MAX bytes
// Unpack: (0x00-success/ret)(AE_SERVO_MAX*1B) // 6+AE_SERVO_MAX bytes
// Time unit is in 20ms!!!
AeRet_t CProtoSerial::PackSetServosAngle(uint32_t &size, uint8_t *data, uint16_t *angles, uint32_t msec, uint32_t lckmsec)
{
	uint8_t len = 8 + AE_SERVO_MAX * 2;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_SET_SERVOS_ANGLE);
	for (int i = 0; i < AE_SERVO_MAX; i++) {
		putU16(pos, data, angles[i]);
	}
	putU8(pos, data, (msec / 20) & 0xFF);
	putU8(pos, data, (lckmsec / 20) & 0xFF);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackSetServosAngle(uint32_t size, const uint8_t *data, AeRet_t *rets)
{
	if (!check(size, data, 6 + AE_SERVO_MAX)) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 4;
	for (int i = 0; i < AE_SERVO_MAX; i++) {
		rets[i] = (AeRet_t)data[pos + i];
	}
	pos += AE_SERVO_MAX;

	return AE_RET_SUCCESS;
}

// Pack: idx(1B) // 7bytes
// Unpack: idx(1B) Angle(2B,0xFF00-Err) // 9bytes
AeRet_t CProtoSerial::PackGetServoAngle(uint32_t &size, uint8_t *data, uint8_t idx)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_GET_SERVO_ANGLE);
	putU8(pos, data, idx);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackGetServoAngle(uint32_t size, const uint8_t *data, uint8_t &idx, uint16_t &angle)
{
	if (!check(size, data, 9)) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 4;
	getU8(pos, data, idx);
	getU16(pos, data, angle);

	return AE_RET_SUCCESS;
}

// Pack: 0x00 // 7bytes
// Unpack: Angles(AE_SERVO_MAX*2B,0xFF00-Err) // 6+2*AE_SERVO_MAX bytes
AeRet_t CProtoSerial::PackGetServosAngle(uint32_t &size, uint8_t *data)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_GET_SERVOS_ANGLE);
	putU8(pos, data, 0x00);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackGetServosAngle(uint32_t size, const uint8_t *data, uint16_t *angles)
{
	if (!check(size, data, 6 + 2 * AE_SERVO_MAX)) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 4;
	for (int i = 0; i < AE_SERVO_MAX; i++) {
		angles[i] = (data[pos + i * 2] << 8) | data[pos + i * 2 + 1];
	}
	pos += (2 * AE_SERVO_MAX);

	return AE_RET_SUCCESS;
}

// Pack: idx(1B) offset(2B:signed) // 9bytes
// Unpack: idx(1B) ret(1B) // 8bytes
AeRet_t CProtoSerial::PackSetServoOffset(uint32_t &size, uint8_t *data, uint8_t idx, int16_t offset)
{
	uint8_t len = 9;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_SET_SERVO_OFFSET);
	putU8(pos, data, idx);
	putU16(pos, data, offset);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackSetServoOffset(uint32_t size, const uint8_t *data, uint8_t &idx)
{
	if (!check(size, data, 8)) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 4;
	idx = data[pos++];

	return (AeRet_t)data[5];
}

// Pack: offsets(AE_SERVO_MAX*2B:signed) // 6+2*AE_SERVO_MAX bytes
// Unpack: (0x00-success/ret)(AE_SERVO_MAX*1B) // 6+AE_SERVO_MAX bytes
AeRet_t CProtoSerial::PackSetServosOffset(uint32_t &size, uint8_t *data, int16_t *offsets)
{
	uint8_t len = 6 + AE_SERVO_MAX * 2;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_SET_SERVOS_OFFSET);
	for (int i = 0; i < AE_SERVO_MAX; i++) {
		putU16(pos, data, offsets[i]);
	}
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackSetServosOffset(uint32_t size, const uint8_t *data, AeRet_t *rets)
{
	if (!check(size, data, 6 + AE_SERVO_MAX)) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 4;
	for (int i = 0; i < AE_SERVO_MAX; i++) {
		rets[i] = (AeRet_t)data[pos + i];
	}
	pos += AE_SERVO_MAX;

	return AE_RET_SUCCESS;
}

// Pack: idx(1B) // 7bytes
// Unpack: idx(1B) offset(2B:0x88xx-err) // 9bytes
AeRet_t CProtoSerial::PackGetServoOffset(uint32_t &size, uint8_t *data, uint8_t idx)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_GET_SERVO_OFFSET);
	data[pos++] = idx;
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackGetServoOffset(uint32_t size, const uint8_t *data, uint8_t &idx, int16_t &offset)
{
	if (!check(size, data, 9)) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 4;
	idx = data[pos++];
	offset = (data[pos] << 8) | data[pos + 1];
	pos += 2;

	return AE_RET_SUCCESS;
}

// Pack: 0x00 // 7bytes
// Unpack: offsets(0x88xx-err)(AE_SERVO_MAX*2B) // 6+2*AE_SERVO_MAX bytes
AeRet_t CProtoSerial::PackGetServosOffset(uint32_t &size, uint8_t *data)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_GET_SERVOS_OFFSET);
	putU8(pos, data, 0x00);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackGetServosOffset(uint32_t size, const uint8_t *data, int16_t *offsets)
{
	if (!check(size, data, 6 + 2 * AE_SERVO_MAX)) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 3;
	for (int i = 0; i < AE_SERVO_MAX; i++) {
		offsets[i] = (data[pos + i * 2] << 8) | data[pos + i * 2 + 1];
	}
	pos += (2 * AE_SERVO_MAX);

	return AE_RET_SUCCESS;
}

// Pack: 0x00(1B) // 7bytes
// Unpack: 0x00(1B) // 7bytes
AeRet_t CProtoSerial::PackLockServos(uint32_t &size, uint8_t *data)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_LOCK_SERVOS);
	putU8(pos, data, 0x00);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackLockServos(uint32_t size, const uint8_t *data)
{
	if (!check(size, data, 7)) {
		return AE_RET_ERROR;
	}

	return AE_RET_SUCCESS;
}

// Pack: 0x00(1B) // 7bytes
// Unpack: 0x00(1B) // 7bytes
AeRet_t CProtoSerial::PackUnlockServos(uint32_t &size, uint8_t *data)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_UNLOCK_SERVOS);
	putU8(pos, data, 0x00);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackUnlockServos(uint32_t size, const uint8_t *data)
{
	if (!check(size, data, 7)) {
		return AE_RET_ERROR;
	}

	return AE_RET_SUCCESS;
}

// Pack: idx(1B) // 7bytes
// Unpack: idx(1B) firmware(4B) // 11bytes
AeRet_t CProtoSerial::PackGetServoFirmware(uint32_t &size, uint8_t *data, uint8_t idx)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_GET_SERVO_FIRMWARE);
	data[pos++] = idx;
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackGetServoFirmware(uint32_t size, const uint8_t *data, uint8_t &idx, uint32_t &firmware)
{
	if (!check(size, data, 11)) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 4;
	idx = data[pos++];
	getU32(pos, data, firmware);

	return AE_RET_SUCCESS;
}

// Pack: 0x00 // 7bytes
// Unpack: firmwares(AE_SERVO_MAX*4B) // 6+4*AE_SERVO_MAX bytes
AeRet_t CProtoSerial::PackGetServosFirmware(uint32_t &size, uint8_t *data)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_GET_SERVOS_FIRMWARE);
	putU8(pos, data, 0x00);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackGetServosFirmware(uint32_t size, const uint8_t *data, uint32_t *firmwares)
{
	if (!check(size, data, 6 + 4 * AE_SERVO_MAX)) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 4;
	for (int i = 0; i < AE_SERVO_MAX; i++) {
		getU32(pos, data, firmwares[i]);
	}

	return AE_RET_SUCCESS;
}

// Pack: name(AE_PATH_MAX) // 6+AE_PATH_MAX bytes
// Unpack: waitMs(4B) // 10bytes
AeRet_t CProtoSerial::PackStartTeach(uint32_t &size, uint8_t *data, uint32_t nameLen, const char *nameStr)
{
	uint8_t len = 6 + AE_PATH_MAX;
	if (size < len) {
		return AE_RET_ERROR;
	}
	if (nameLen >= AE_PATH_MAX) {
		return AE_RET_PARAMS;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_START_TEACHING);
	putString(pos, data, nameLen, nameStr);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackStartTeach(uint32_t size, const uint8_t *data, uint32_t &waitMs)
{
	if (!check(size, data, 10)) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 4;
	getU32(pos, data, waitMs);

	return AE_RET_SUCCESS;
}

// Pack: 0x00 // 7bytes
// Unpack: ret(1B) // 7bytes
AeRet_t CProtoSerial::PackStopTeach(uint32_t &size, uint8_t *data)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_STOP_TEACHING);
	putU8(pos, data, 0x00);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackStopTeach(uint32_t size, const uint8_t *data)
{
	if (!check(size, data, 7)) {
		return AE_RET_ERROR;
	}

	return (AeRet_t)data[4];
}

// Pack: angles(2*AE_SERVO_MAX) // 6+2*AE_SERVO_MAX bytes
// Unpack: None
AeRet_t CProtoSerial::PackTeachSending(uint32_t &size, uint8_t *data, uint16_t *angles)
{
	uint8_t len = 6 + 2 * AE_SERVO_MAX;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_TEACH_SENDING);
	for (int i = 0;i < AE_SERVO_MAX;i++) {
		putU16(pos, data, angles[i]);
	}
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackTeachSending(uint32_t size, const uint8_t *data, uint16_t *angles)
{
	if (!check(size, data, 6 + 2 * AE_SERVO_MAX)) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 4;
	for (int i = 0; i < AE_SERVO_MAX; i++) {
		getU16(pos, data, angles[i]);
	}

	return AE_RET_SUCCESS;
}

// Pack: move(1B) // 7bytes
// Unpack: move(1B) // 7bytes
AeRet_t CProtoSerial::PackMoving(uint32_t &size, uint8_t *data, uint8_t move)
{
	uint8_t len = 7;
	if (size < len) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 0;
	putHeader(pos, data, len, AE_PCMD_MOVING);
	putU8(pos, data, move);
	putTail(pos, data, len);

	size = len;
	return AE_RET_SUCCESS;
}

AeRet_t CProtoSerial::UnpackMoving(uint32_t size, const uint8_t *data, uint8_t &move)
{
	if (!check(size, data, 7)) {
		return AE_RET_ERROR;
	}

	uint32_t pos = 4;
	getU8(pos, data, move);

	return AE_RET_SUCCESS;
}

void CProtoSerial::putU8(uint32_t &offset, uint8_t *data, uint8_t val)
{
	data[offset++] = val;
}

void CProtoSerial::putU16(uint32_t &offset, uint8_t *data, uint16_t val)
{
	data[offset++] = (val >> 8) & 0xFF;
	data[offset++] = val & 0xFF;
}

void CProtoSerial::putU32(uint32_t &offset, uint8_t *data, uint32_t val)
{
	data[offset++] = (val >> 24) & 0xFF;
	data[offset++] = (val >> 16) & 0xFF;
	data[offset++] = (val >> 8) & 0xFF;
	data[offset++] = val & 0xFF;
}

void CProtoSerial::putString(uint32_t &offset, uint8_t *data, uint32_t len, const char *str)
{
	memset(data + offset, 0x00, AE_PATH_MAX);
	memcpy(data + offset, str, len);
	offset += AE_PATH_MAX;
}

void CProtoSerial::putHeader(uint32_t &offset, uint8_t *data, uint8_t len, uint8_t cmd)
{
	data[offset++] = 0xFB;
	data[offset++] = 0xBF;
	data[offset++] = len;
	data[offset++] = cmd;
}

void CProtoSerial::putTail(uint32_t &offset, uint8_t *data, uint8_t len)
{
	data[offset++] = Checksum(len, data);
	data[offset++] = 0xED;
}

bool CProtoSerial::check(uint32_t size, const uint8_t *data, uint32_t len)
{
	if (size < len) {
		return false;
	}

	uint8_t sum = Checksum(size, data);
	if (0xFB != data[0] || 0xBF != data[1] || sum != data[size - 2] || 0xED != data[size - 1]) {
		return false;
	}

	return true;
}

void CProtoSerial::getU8(uint32_t &offset, const uint8_t *data, uint8_t &val)
{
	val = data[offset++];
}

void CProtoSerial::getU16(uint32_t &offset, const uint8_t *data, uint16_t &val)
{
	val = (data[offset] << 8) | data[offset + 1];
	offset += 2;
}

void CProtoSerial::getU32(uint32_t &offset, const uint8_t *data, uint32_t &val)
{
	val = (data[offset] << 24) | (data[offset + 1] << 16) | (data[offset + 2] << 8)
		| data[offset + 3];
	offset += 4;
}

void CProtoSerial::getString(uint32_t &offset, const uint8_t *data, char *str)
{
	memset(str, 0x00, AE_PATH_MAX);
	memcpy(str, data + offset, AE_PATH_MAX);
	offset += AE_PATH_MAX;
}

